﻿#include "CameraTool.h"
#include <stdio.h>
#include <turbojpeg.h>
#include <memory>
#include <algorithm>

#pragma comment(lib,"turbojpeg-static.lib")

/** rgb格式转yuv格式 */
int RgbToYuv(unsigned char* RgbBuf,
    unsigned int nWidth, unsigned int nHeight,
    unsigned char* yuvBuf, unsigned long* len)
{
    if (RgbBuf == nullptr)
    {
        return -1;
    }

    int i, j;
    unsigned char* bufY, * bufU, * bufV, * bufRGB;
    memset(yuvBuf, 0, (unsigned int)*len);
    bufY = yuvBuf;
    bufV = yuvBuf + nWidth * nHeight;
    bufU = bufV + (nWidth * nHeight * 1 / 4);

    *len = 0;
    unsigned char y, u, v, r, g, b;
    unsigned int ylen = nWidth * nHeight;
    unsigned int ulen = (nWidth * nHeight) / 4;
    unsigned int vlen = (nWidth * nHeight) / 4;

    for (j = 0; j < nHeight; j++)
    {
        bufRGB = RgbBuf + nWidth * (nHeight - 1 - j) * 3;
        for (i = 0; i < nWidth; i++)
        {
            int pos = nWidth * i + j;

            r = *(bufRGB++);
            g = *(bufRGB++);
            b = *(bufRGB++);

            y = (unsigned char)((66 * r + 129 * g + 25 * b + 128) >> 8) + 16;
            u = (unsigned char)((-38 * r - 74 * g + 112 * b + 128) >> 8) + 128;
            v = (unsigned char)((112 * r - 94 * g - 18 * b + 128) >> 8) + 128;

            *(bufY++) = std::max(0, std::min((int)y, 255));

            if (j % 2 == 0 && i % 2 == 0)
            {
                if (u > 255)
                {
                    u = 255;
                }
                if (u < 0)
                {
                    u = 0;
                }

                // 存u分量
                *(bufU++) = u;
            }
            else
            {
                // 存v分量  
                if (i % 2 == 0)
                {
                    if (v > 255)
                    {
                        v = 255;
                    }
                    if (v < 0)
                    {
                        v = 0;
                    }
                    *(bufV++) = v;
                }
            }
        }
    }
    *len = nWidth * nHeight + (nWidth * nHeight) / 2;

    return 0;
}

/** jpeg格式转yuv格式 */
int JpegToYuv(unsigned char* jpeg_buffer, int jpeg_size, 
    unsigned char** yuv_buffer, int width, int height)
{
    int flags = 0;
    int padding = 1; // 1或4均可，但不能是0

    tjhandle handle = tjInitDecompress();

    int ret = tjDecompressToYUV2(handle, jpeg_buffer, jpeg_size, *yuv_buffer, width, padding, height, flags);

    if (ret < 0)
    {
        tjDestroy(handle);
        return -1;
    }

    tjDestroy(handle);

    return ret;
}

/** yuyv422转换为yuv420 */
void yuyv422_to_yuv420(unsigned char* in, unsigned char* out, int width, int height)
{
    unsigned char *y, *u, *v;
    int index_y, index_u, index_v;
    int i, j;

    y = out;
    u = out + width * height;
    v = out + width * height * 5 / 4;

    index_y = 0;
    index_u = 0;
    index_v = 0;

    // 4个分量读2个像素, 所以读出一行像素需要宽度乘2
    int dataWidth = width * 2;

    for (j = 0; j < height; j++)
    {
        // 读取每行数据, 以4个分量为一个数据单位
        for (i = 0; i < dataWidth; i = i + 4)
        {
            // 2个分量取一个y, 一个数据单位取两个y
            *(y + (index_y++)) = *(in + dataWidth * j + i);
            *(y + (index_y++)) = *(in + dataWidth * j + i + 2);

            // 每两行取一行u和v
            if (j % 2 == 0)
            {
                // 4个分量取一次u, 取分量第2个元素
                *(u + (index_u++)) = *(in + dataWidth * j + i + 1);

                // 4个分量取一次v, 取分量第4个元素
                *(v + (index_v++)) = *(in + dataWidth * j + i + 3);
            }
        }
    }
}

/** yuv422转换为yuv420 */
void yuv422_to_yuv420(unsigned char* in, unsigned char* out, int width, int height)
{
    unsigned char* y, * u, * v;
    int index_u, index_v;
    int i, j;

    y = out;
    u = out + width * height;
    v = out + width * height * 5 / 4;

    memcpy(y, in, width * height);

    index_u = 0;
    index_v = 0;

    int u_location = width * height;
    int v_location = width * height * 3 / 2;
    int dataWidth = width / 2;

    // 每两行取一行u和v
    for (j = 0; j < height; j = j + 2)
    {
        for (i = 0; i < dataWidth; i++)
        {
            *(u + (index_u++)) = *(in + u_location + dataWidth * j + i);
            *(v + (index_v++)) = *(in + v_location + dataWidth * j + i);
        }
    }
}
